#扩展：4. 使用光流估计方法，在前述测试视频上计算特征点，进一步进行特征点光流估计。

import cv2
import numpy as np

cap = cv2.VideoCapture('vtest.avi')
# 角点检测所需参数,在进行光流估计计算之前，需要先进行角点检测，然后再把检测出的特征点进行光流估计。
feature_params = dict(maxCorners=100,
                      qualityLevel=0.3,
                      minDistance=7)
# lucas kanade参数
lk_params = dict(winSize=(15, 15),
                 maxLevel=2)
# 随机颜色条
color = np.random.randint(0, 255, (100, 3))
# 拿到第一帧图像，这里没有循环。
ret, old_frame = cap.read()
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
# 返回所有检测特征点，第一个参数输入图像，maxCorners：角点最大数量（效率），qualityLevel：品质因子（特征值越大的越好，来筛选）
# minDistance：距离，相当于这区间有比这个角点强的，就不要这个弱的了。
P0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **feature_params)  # 获取图像中最好的角点特征,None表示在整幅图上寻找角点。
print(P0.shape)  #
# 创建一个mask
mask = np.zeros_like(old_frame)

while (True):
    ret, frame = cap.read()
    frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    # 需要传入前一帧和当前图像以及前一帧检测到的角点
    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, P0, None, **lk_params)
    good_new = p1[st == 1]  # st=1当前帧图像检测到了上一阵的角点特征,shape为（38,2）,2表示坐标值
    good_old = P0[st == 1]
    # 绘制轨迹
    for i, (new, old) in enumerate(zip(good_new, good_old)):
        a, b = new.ravel()  # 新坐标
        c, d = old.ravel()  # 旧坐标
        mask = cv2.line(mask, (a, b), (c, d), color[i].tolist(), 2)  # 直线的起点（a，b），直线的终点坐标（c，d）
        frame = cv2.circle(frame, (a, b), 5, color[i].tolist(), -1)
    img = cv2.add(frame, mask)

    cv2.imshow('frame', img)
    k = cv2.waitKey(150) & 0xff
    if k == 27:
        break

    # 更新
    old_gray = frame_gray.copy()
    P0 = good_new.reshape(-1, 1, 2)

cv2.destroyAllWindows()
cap.release()